/*
 * "Copyright (c) 2012 Universitat Pompeu Fabra.
 * All rights reserved.
 *
 * Permission to use, copy, modify, and distribute this software and its
 * documentation for any purpose, without fee, and without written agreement is
 * hereby granted, provided that the above copyright notice, the following
 * two paragraphs and the author appear in all copies of this software.
 *
 * IN NO EVENT SHALL UNIVERSITAT POMPEU FABRA BE LIABLE TO ANY PARTY 
 * FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING 
 * OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF POMPEU FABRA
 * HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * UNIVERSITAT POMPEU FABRA SPECIFICALLY DISCLAIMS ANY WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
 * AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND UNIVERSITAT POMPEU FABRA HAS NO OBLIGATION TO 
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS."
 */

#include "HbcpNetworkPackets.h"
#include "HbcpStatistics.h"
#include "HbcpRoutingEngine.h"

module HbcpRoutingEngineP {
	provides {
		interface HbcpRouting as Routing;
		interface HbcpNetworkManager as NetworkManager;
		interface Init;
	}
	uses {
		interface Timer<TMilli> as TimerGST; //Group Slot Time
		interface CC2420Packet as Packet;
		interface Receive as RoutingEvent;
		interface Read<uint16_t> as BatteryReader;
		interface Random;
		interface Leds;
	}
}
implementation {

	routingPacket_t routingPacket;
	routingPacket_t announcePacket;

	bool processingFlag = FALSE;
	uint8_t currentState = INITIAL_STATE;
	
	uint8_t candidateLinkQuality;	
	uint8_t localBattery;

	//C information
	int8_t currentRSSI;
	uint8_t currentBattery;
	uint8_t currentHop, extendedBaseHop;
	uint16_t currentSession;
	uint8_t currentLinkQuality;
	uint16_t currentParent;

	/********************************* COMMANDS ********************************/ 

	command error_t Init.init() {
		currentState = INITIAL_STATE;
		currentLinkQuality = 0;
		processingFlag = FALSE;
		return SUCCESS;
	}

	command uint8_t Routing.getCurrentHop() {
		return currentHop;
	}

	command uint8_t Routing.getBattery() {
		return currentBattery;
	}

	command am_addr_t Routing.getNextHop() {
		return currentParent;
	}

	command void Routing.allowRoutingEngine(bool state) {
		allowRouting = state;
	}

	//Read the actual battery level of the node.
	event void BatteryReader.readDone(error_t result, uint16_t val) {
		if(result != SUCCESS) {
			val = 0x0000;
		}
		localBattery = (uint8_t)(((double)(val - 2048) / (double) 2048) * 100);
	}

	/********************************* FUNCTIONS ***************************************/ 

	bool isReset(routingPacket_t * rPacket) {
		if(rPacket->session == 0) {
			return TRUE;
		}
		return FALSE;
	}

	void setCandidateAsParent(message_t * msg) {
		currentLinkQuality = candidateLinkQuality;
		currentSession = routingPacket.session;
		currentParent = routingPacket.generalFrame.source;
		currentHop = routingPacket.hop + 1;
	}

	void processInitialRoutingPacket(message_t * msg) {	
		currentState = STATE_ACTIVE_TGS;
		call TimerGST.startOneShot(routingPacket.coordinationTime);
		setCandidateAsParent(msg);
	}

	void evaluatePackets(message_t * msg) {
		if(currentLinkQuality < candidateLinkQuality) {
			setCandidateAsParent(msg);
		}
	}

	routingPacket_t * createNewDiscoveryPacket() {

		uint16_t sst;
		
		call BatteryReader.read();
	
		sst = (uint16_t)(((double)(call Random.rand16() % DISCOVERY_SLOT_TIME)) * 0.8);

		announcePacket.generalFrame.networkID = HBTP_TYPE;
		announcePacket.generalFrame.source = TOS_NODE_ID;
		announcePacket.generalFrame.type = ROUTING_TYPE;
		announcePacket.generalFrame.length = sizeof(routingPacket_t);

		announcePacket.battery = localBattery;
		announcePacket.coordinationTime = DISCOVERY_SLOT_TIME - sst;
		
		if(currentLinkQuality > RSSI_QUALITY){
			announcePacket.hop = currentHop;		
		}
		else{
			announcePacket.hop = extendedBaseHop+1;//With the current hop is not always true
		}
		
		//Update metrics
		announcePacket.maxHops = MAX_HOPS;
		announcePacket.groupSlotTime = DISCOVERY_SLOT_TIME;
		announcePacket.collectionTime = COLLECTION_SLOT_TIME;
		announcePacket.thresholdRssi = RSSI_DROP;
		announcePacket.dangerRssi = RSSI_QUALITY;
		announcePacket.transmissionPower = TX_POWER;
		announcePacket.session = currentSession;

		//TODO: Review line
		announceLength = sizeof(routingPacket_t);
		
		//statistics
		stats.battery = localBattery;
		stats.hops = currentHop;
		stats.linkQuality = currentLinkQuality;
		stats.nextHop = currentParent;
		
		return &announcePacket;
	}

	int8_t calculateLinkQuality(message_t * msg, routingPacket_t * rPacket) {
			
		if(rPacket->battery > 15){
			return (call Packet.getRssi(msg) + RSSI_OFFSET_CC2420);
		}
		else{
			if(call Packet.getRssi(msg) < RSSI_QUALITY){
				return RSSI_DROP;		
			}
			return RSSI_QUALITY; 
		}
	}

	/******************************* RECEIVE PACKET ************************************/ 

	event message_t * RoutingEvent.receive(message_t * msg, void * payload,
			uint8_t len) {

		if(allowRouting && ! processingFlag) {

			call Leds.led1On();
			processingFlag = TRUE;

			memcpy(&routingPacket, payload, len);
						
			//Check if it is a network reset packet
			if(isReset(&routingPacket)) {
				signal NetworkManager.routingSignal(&routingPacket, len);
				return msg;
			}
			
			//Update metrics
			MAX_HOPS = routingPacket.maxHops;
			DISCOVERY_SLOT_TIME = routingPacket.groupSlotTime;
			COLLECTION_SLOT_TIME = routingPacket.collectionTime;
			RSSI_DROP = routingPacket.thresholdRssi;
			RSSI_QUALITY = routingPacket.dangerRssi;
			TX_POWER = routingPacket.transmissionPower;
			
			//If it has reached tha max hops or doesn't comply with rssi quality criteria
			if((MAX_HOPS <= routingPacket.hop) || ((call Packet.getRssi(msg) + RSSI_OFFSET_CC2420) < RSSI_DROP)) {
				processingFlag = FALSE;
				call Leds.led1Off();
				return msg;
			}	
			
			//Get the node's link quality
			candidateLinkQuality = calculateLinkQuality(msg,&routingPacket);
			
			if(currentState == INITIAL_STATE) {
				processInitialRoutingPacket(msg);
			}				
			else {
				evaluatePackets(msg);
			}
			processingFlag = FALSE;
		}
		return msg;
	}

	/*********************************** TIMERS *************************************/ 

	//Fired when the announcing Group Slot Time for nodes from previous hop is finished
	//Decides to announce the node as new route or wait an extended period
	event void TimerGST.fired() {
		
		processingFlag = TRUE;
		if(currentLinkQuality > (RSSI_QUALITY) || currentState == STATE_EXTENDED_TGS){	
			currentState = INITIAL_STATE;
			signal NetworkManager.routingSignal(createNewDiscoveryPacket(), announceLength);
		}
		else {
			currentState = STATE_EXTENDED_TGS;
			extendedBaseHop = currentHop;
			stats.extendedTGS = 1;
			call TimerGST.startOneShot(DISCOVERY_SLOT_TIME);
		}
		processingFlag = FALSE;
	}
}